#include "libmri.h"
#include <omp.h>

void calib2d_orthiter (complex *vmap, float *emap, int *imsize, complex *Km,
                       int qcols, int qriters)
{
	int im1 = imsize[0], im2 = imsize[1], nch = imsize[2];

	complex *A = new complex [nch*nch];
	complex *Q = new complex [nch*qcols];
	complex *T = new complex [nch*qcols];
	complex *v = new complex [qcols];

	complex _1[1] = {{1.0f,0.0f}}, _0[1] = {{0.0f, 0.0f}},
	        _n1[1] = {{-1.0f, 0.0f}};

	acmlsetnumthreads(1);
	omp_set_num_threads(1);

	for (int iy = 0; iy < im2; ++iy)
		for (int ix = 0; ix < im1; ++ix)
		{
			for (int i = 0; i < nch*nch; ++i)
				A[i] = Km [ix + im1*(iy + im2*i)];

			for (int i = 0; i < nch*qcols; ++i)
				Q[i].real = Q[i].imag = 0.0f;

			for (int i = 0; i < qcols; ++i)
				Q[i + nch*i].real = 1.0f;

			for (int iter = 1; iter <= qriters; ++iter)
			{
				cgemm ('N','N', nch, qcols, nch, _1, A,nch, Q,nch, _0, T,nch);

				for (int j = 0; j < qcols; ++j){
					complex nrm = {1.0f/scnrm2(nch, &T[nch*j], 1), 0.0f};
					cscal (nch, &nrm, &T [nch*j], 1);
				}

				for (int j = 1; j < qcols; ++j){
					cgemv ('C', nch, j,  _1, T,nch, &T[j*nch],1, _0, v,1);
					cgemv ('N', nch, j, _n1, T,nch, v,1, _1, &T[j*nch],1);
				}

				complex *tmp = Q; Q = T; T = tmp;
			}

			cgemm ('N', 'N', nch, qcols, nch, _1, A, nch, Q, nch, _0, T, nch);

			for (int j = 0; j < qcols; ++j){
				emap[ix + im1*(iy + im2*j)] = scnrm2 (nch, &T[nch*j],1);
			}

			for (int i = 0; i < qcols*nch; ++i){
				vmap[ix + im1*(iy +im2*i)] = Q[i];
			}
		}

	delete [] A;
	delete [] Q;
	delete [] T;
	delete [] v;
}

